//
//  ahrs.h
//  KFilter
//
//  Created by Kaiwen Gu on 6/23/13.
//  Copyright (c) 2013 com.mika. All rights reserved.
//
#include "ukf.h"

#ifndef KFilter_ahrs_h
#define KFilter_ahrs_h

#define GX 0
#define GY 1
#define GZ 2

#define AX 3
#define AY 4
#define AZ 5

#define OBSx 0
#define OBSy 1
#define OBSz 2

#define EXOINPUTDIM 6

#define AHRS_TIMESTAMP 0.022488f


enum AHRS_outputFormat
{
	AHRS_OUT_QUATERNION,
	AHRS_OUT_EULER,
	AHRS_OUT_ROTATIONMATRIX
};
typedef struct AHRS_CalibrationParameters
{
	/*Scale and cross axis sensitivity*/
	float sc00, sc01, sc02;
	float sc10, sc11, sc12;
	float sc20, sc21, sc22;

	/*Initial bias*/
	float b0,b1,b2;

}AHRS_CalibrationParameters;


typedef struct AHRS_outputModel {
	float yaw;
	float pitch;
	float roll;
	float vx,vy,vz;
	float px, py, pz;
	float longitutude,latitude,altitude;

	float q0,q1,q2,q3;


	float dcm11,dcm12,dcm13;
	float dcm21,dcm22,dcm23;
	float dcm31,dcm32,dcm33;
}AHRS_outputModel;

typedef struct AHRS_dataModel
{
	/*Magnetometer reference frame values*/
	float mag0rf,mag1rf,mag2rf;

}AHRS_dataModel;

int AHRSInit();
void processInertialSensors(float ax, float ay, float az, float gx, float gy, float gz);
void predictionUpdate(float * inState, float * outState, float * noise, float * exogenousInput, float dt);
void processAccelerometerObservation(float ax, float ay, float az);
void processBarometricPressureObservation(float p);

void accelerometerObservationUpdate(float * inState, float * outState, float * noise, float * exogenousInput);
void magnetometerObservationUpdate(float * inState, float * outState, float * noise, float * exogenousInput);
void barameterObservationUpdate(float * inState, float * outState, float * noise, float * exogenousInput);


void processOutput(uint8_t AHRS_outputType);

extern AHRS_outputModel AHRSOutputModel;

#endif
